#include "can1.h"
#include "task.h"
#include "user.h"
#include "delay.h"
#include "uart1.h"
#include "usart2.h"
#include "uart3.h"
#include "pwm.h"
#include "imu.h"
#include "gpio.h"
#include "includes.h"

//                                 kp   ki  kd        error[3]  maxErr outPut maxOut
PID_ABS_t pid_bodan_208_velo = {8.5f, 0.05f, 0.0f, {0.0f, 0.0f, 0.0f}, 10000.0f, 0.0f, 3000.0f};

#define max_heat 280;

extern OS_TMR tmr1;
//extern float output_208;
extern float this_205_angle;
extern float this_206_angle;
extern float this_208_angle;
extern float target_pitch_angle;
extern float target_yaw_angle;
u8 fire_flag = 1;	  //��ؿ�����־λ
u8 continue_flag = 1; //����������־λ
extern uint8_t start_fire_flag;
extern uint16_t shooter_heat;
extern u8 NewData_flag;
extern u8 shoot_flag;
extern u8 stop_flag;
extern u8 friction_flag;
extern int time_1s;
extern u8 shoot_outpost_flag;
extern u8 gimbal_ok_flag;

static float start_pos = 0, last_pos = 0;
int not_jam = 1;
u8 timing_flag2 = 0;

void turnback_processing(int16_t this_pos);
void auto_mode(void);
void manual_mode(void);
void start_shoot(int shoot_speed);
void stop_shoot(void);
void shoot_5s(void);

void Mode_task(void *p_arg)
{
	OS_ERR err;
	p_arg = p_arg;

	// u8 roundtime = 0;
	while (1)
	{
#ifdef up_gimbal
		if (TeleCtrl_Rev_OK < 5)
#endif
#ifdef down_gimbal
			//if (start_fire_flag == 4)
			if (TeleCtrl_Rev_OK < 5)
#endif
			{
#ifdef up_gimbal
				switch (TelCtrlData.switch_r)
				{
				case switch_up:
					manual_mode();
					timing_flag2 = 0;
					break;

				case switch_middle:
					if (shoot_outpost_flag == 0 && gimbal_ok_flag == 1)
						shoot_5s();
					else if (shoot_outpost_flag == 1)
						auto_mode();
					break;
				}
#endif
#ifdef down_gimbal
				auto_mode();
				//manual_mode();
#endif
			}
			else
			{
				timing_flag2 = 0;
				pid_bodan_208_velo.Output = 0;
			}
		Bodan_ESC_Send(pid_bodan_208_velo.Output);

		OSTimeDly(10, OS_OPT_TIME_PERIODIC, &err); //10ms
	}
}

void tmr1_callback(void *p_tmr, void *p_arg)
{
	continue_flag = 1;
}

void turnback_processing(int16_t this_pos)
{
	do
	{
		pid_bodan_208_velo.Output = 700;
		delay_ms(400);
		pid_bodan_208_velo.Output = -800;
		delay_ms(400);
		if (this_pos < encoder_208 - 320 || (8191 - encoder_208) + this_pos > 320)
			return;
	} while (1);
}

void auto_mode()
{
	if (shoot_flag && friction_flag && start_fire_flag == 4)
		start_shoot(2700);
	else
		stop_shoot();
}

void manual_mode()
{
	if (TelCtrlData.switch_l == switch_down)
		start_shoot(2700);
	else
		stop_shoot();
}

void start_shoot(int shoot_speed)
{
	if (not_jam && shooter_heat <= 280)
	{
#ifdef up_gimbal
		PID_Calc(&pid_bodan_208_velo, velocity_208, -shoot_speed);
#endif
#ifdef down_gimbal
		PID_Calc(&pid_bodan_208_velo, velocity_208, shoot_speed);
#endif
	}
	else
	{
		PID_Calc(&pid_bodan_208_velo, velocity_208, 0);
	}
}

void stop_shoot()
{
	PID_Calc(&pid_bodan_208_velo, velocity_208, 0);
}

void shoot_5s()
{
	if (friction_flag == 1 && start_fire_flag == 4)
	{
		if (timing_flag2 == 0)
		{
			timing_flag2 = 1;
			time_1s = 0;
		}
		if (timing_flag2 == 1 && time_1s <= 10)
		{
			start_shoot(2700);
		}
		else
		{
			shoot_outpost_flag = 1;
		}
	}
}
